package catchnthrow;

import java.io.File;
import java.io.FileWriter;
import java.io.IOException;
import java.text.DateFormat;
import java.util.*;

import se.lth.control.realtime.*;

public class BallAndBeamRegul extends Thread {    

	private PID inner;
	private PID outer;

	public final static int unknownBall = 3;	
	public final static int smallBall = 0;
	public final static int mediumBall = 1;
	public final static int bigBall = 2;

	private final double minAngle, maxAngle, minPos, maxPos;

//	Real IOs
	private AnalogIn analogInAngle; 
	private AnalogIn analogInPosition; 
	private AnalogOut analogOut;

	private long starttime;

	//Monitors
	private YBufferMonitor yBufMon;
	private static final int yBuffersize = 100;
	private IsRunningMonitor isRunMon;
	private RefMonitor refMon;
	
	private EstimateMonitor estiMonU;
	private static final double toleranceU = 0.1;
	private static final double meanBigBall = -2.8;
	private static final double meanMediumBigBall = -1.4;
//	private static final double meanMediumSmallBall = -0.8;
	private static final double meanSmallBall = -0.5;
	
	
	private ShouldTrackMonitor shouldEstiMon;

	private SM.uPlotMonitor uPlotMon;
	private final boolean plotU;

	public BallAndBeamRegul(AnalogIn inPosition, AnalogIn inAngle, AnalogOut outMotor, int pri) {
		setPriority(pri);
		plotU = false;

		yBufMon = new YBufferMonitor(yBuffersize);
		isRunMon = new IsRunningMonitor(false);
		refMon = new RefMonitor(0.0);	
		estiMonU = new EstimateMonitor();
		shouldEstiMon = new ShouldTrackMonitor(false);

		analogInPosition = inPosition; 
		analogInAngle = inAngle; 
		analogOut = outMotor;

		minAngle = -10;
		maxAngle = 10;
		minPos = -10;
		maxPos = 10;
	}

	public BallAndBeamRegul(AnalogIn inPosition, AnalogIn inAngle, AnalogOut outMotor,
			SM.uPlotMonitor uMonitor, int pri) {

		setPriority(pri);
		plotU = true;

		yBufMon = new YBufferMonitor(yBuffersize);
		isRunMon = new IsRunningMonitor(false);
		refMon = new RefMonitor(0.0);
		estiMonU = new EstimateMonitor();
		shouldEstiMon = new ShouldTrackMonitor(false);

		analogInPosition = inPosition; 
		analogInAngle = inAngle; 
		analogOut = outMotor;

		uPlotMon = uMonitor;

		minAngle = -10;
		maxAngle = 10;
		minPos = -10;
		maxPos = 10;
	}

	public void setRef(double r){
		refMon.setRef(r);
	}

	public synchronized void setInnerParameters(PIDParameters p){
		if (inner == null) {
			inner = new PID("innerAngle", p);
		} else {
			inner.setParameters(p);
		}
	} 

	public synchronized void setOuterParameters(PIDParameters p) {
		if (outer == null) {
			outer = new PID("outerBall", p);
		} else {
			outer.setParameters(p);
		}
	}

	public void run() {
		int yIter = 0; //Iterator for yBuffer
		isRunMon.setShouldRun(true);
		double yRef, yAngle, yPos, vi, vo, u;
		long duration;
		long t=System.currentTimeMillis();
		starttime=t;

		while(true){
			if(isRunMon.isShouldRun()){
				yRef = refMon.getRef();
				try {
					yAngle = analogInAngle.get();
					yPos = analogInPosition.get();
				} catch (Exception e1) {
					e1.printStackTrace();
					return;
				}

				vo = outer.calculateOutput(yPos, yRef);
				vo = limit(vo, minPos, maxPos);

				vi = inner.calculateOutput(yAngle, vo);
				u = limit(vi, minAngle, maxAngle);
				try {
					analogOut.set(u);
				} catch (Exception e) {
					e.printStackTrace();
				}

				inner.updateState(u);
				outer.updateState(vo);

				if(plotU) {
					uPlotMon.setU(u);
				}

				//Filling yBuffer for tracking
				yBufMon.setYBufferEntry(yIter, yPos);
				if (yIter < yBuffersize-1) {
					yIter++;
				} else {
					yIter = 0;
				}

				//Filling Buffers for estimating
				if (shouldEstiMon.isShouldtrack()) {
					estiMonU.addEntry(u);
				}
			}
			// sleep
			t = t + inner.getHMillis();
			duration = t - System.currentTimeMillis();
			if (duration > 0) {
				try {
					sleep(duration);
				} catch (Exception x) {
				}
			}
		}
	}

	public boolean hasReached(double error) {
		double yMax = 0;
		double[] yBuffer = yBufMon.getYBuffer();
		double ref = refMon.getRef();

		for (int i = 0; i < yBuffersize; i++) {
			if (Math.abs(yBuffer[i] - ref) > yMax) {
				yMax = yBuffer[i];
			}
		}

		if (yMax <= ref+error && yMax >= ref-error) {
			return true;
		} else {
			return false;
		}
	}

	public boolean isRunning(){
		return isRunMon.isShouldRun();
	}

	public void restart(){
		isRunMon.setShouldRun(true);
	}

	public void trackBall(boolean bool){
		shouldEstiMon.setShouldtrack(bool);
	}
	
	public void saveTrackedBallToFile(String path){
		LinkedList<Double> bufferU = estiMonU.getBuffer();
		
		Calendar cal = Calendar.getInstance();
	    DateFormat formater = DateFormat.getTimeInstance(DateFormat.MEDIUM);
		
		String filename = formater.format(cal.getTime())+".txt";
		try {
			File file = new File(path+"U_"+filename);
			file.createNewFile();
			FileWriter fw = new FileWriter(file);
			
			for (int i = 0; i < bufferU.size(); i++) {
				fw.write(bufferU.get(i).toString());
				fw.write(", ");
			}
			fw.close();
			
		} catch (IOException e) {
			e.printStackTrace();
		}
		estiMonU.getBuffer().clear();

	}
	
	public int ballSize(){
		double meanVal = 0;
		int ballSize;
		LinkedList<Double> bufferU = estiMonU.getBuffer();
		
		//To prevent Zero-devision
		if(bufferU.size() == 0){
			return unknownBall;
		}
		
		//Calculating mean value of U
		for (int i = 0; i < bufferU.size(); i++) {
			meanVal = meanVal + bufferU.get(i);
		}
		meanVal = meanVal/bufferU.size();
		
		//Decision
		if (inBetween(meanVal, meanBigBall, toleranceU)) {
			ballSize = bigBall;
		} else if (inBetween(meanVal, meanSmallBall, toleranceU)){
			ballSize = smallBall;
		} else if (inBetween(meanVal, meanMediumBigBall, toleranceU)){
			ballSize = mediumBall;
		} else {
			ballSize = unknownBall;
		}
		
		//Clearing all old buffer values
		bufferU.clear();
		return ballSize;
	}
	
	private boolean inBetween(double val, double mean, double tol){
		return (val >= mean-tol && val <= mean+tol);
	}

	//	Called from Opcom when shutting down
	public void shutDown(){
		isRunMon.setShouldRun(false);
		inner.reset();
		outer.reset();
		try {
			analogOut.set(0.0);
		} catch (Exception x) {
			x.printStackTrace();
		}
	}

	private double limit(double v, double min, double max) {
		if (v < min) {
			v = min;
		} else {
			if (v > max) {
				v = max;
			}
		}
		return v;
	}

	private class YBufferMonitor{

		private double[] yBuffer;

		public YBufferMonitor(int size) {
			yBuffer = new double[size];
		}

		public synchronized double[] getYBuffer() {
			return yBuffer;
		}

		public synchronized void setYBuffer(double[] buffer) {
			yBuffer = buffer;
		}

		public synchronized void setYBufferEntry(int i, double entry) {
			yBuffer[i] = entry;
		}
	}

	private class IsRunningMonitor{
		private boolean shouldRun;

		public IsRunningMonitor(boolean init){
			shouldRun = init;
		}

		public synchronized boolean isShouldRun() {
			return shouldRun;
		}

		public synchronized void setShouldRun(boolean shouldRun) {
			this.shouldRun = shouldRun;
		}
	}

	private class RefMonitor{
		private double ref;

		public RefMonitor(double init){
			ref = init;
		}

		public synchronized double getRef() {
			return ref;
		}

		public synchronized void setRef(double ref) {
			this.ref = ref;
		}
	}

	private class EstimateMonitor{ 
		private LinkedList<Double> buffer;

		public EstimateMonitor(){
			buffer = new LinkedList<Double>();
		}

		public synchronized LinkedList<Double> getBuffer() {
			return buffer;
		}

		public synchronized void setBuffer(LinkedList<Double> buffer) {
			this.buffer = buffer;
		}
		
		public synchronized void addEntry(double entry) {
			buffer.add(entry);
		}
	}

	private class ShouldTrackMonitor{
		private boolean shouldTrack;

		public ShouldTrackMonitor(boolean init){
			shouldTrack = init;
		}

		public synchronized boolean isShouldtrack() {
			return shouldTrack;
		}

		public synchronized void setShouldtrack(boolean shouldtrack) {
			
			this.shouldTrack = shouldtrack;
		}
	}

}
